#!/usr/bin/env python3
# -*- coding:utf-8 -*-      

import rospy 
import math
import numpy as np
from radio_serial.msg import Cmd_send, Uav_state, FWstates, Leaderstates

STATE_UAV = 101
CMD_UAV = 102
ARM = 1
TAKEOFF = 2
LAND = 3
POS = 4
VEL = 5
ACC = 6
POS_VEL = 7
POS_ACC = 8
VEL_ACC = 9
POS_VEL_ACC = 10

leader_id = "plane_0"
vir_leader_state = Leaderstates()
    
def uav0_state_cb(msg:Uav_state):
    # 这里cmd接收到的信息是虚拟领导者的期望位置和期望姿态
    global vir_leader_state
    
    vir_leader_state.ned_pos_x = msg.local_x
    vir_leader_state.ned_pos_y = msg.local_y
    vir_leader_state.ned_pos_z = msg.local_z
    
    vir_leader_state.ned_vel_x = msg.local_lin_x
    vir_leader_state.ned_vel_y = msg.local_lin_y
    vir_leader_state.ned_vel_z = msg.local_lin_z
    
    airspeed = math.sqrt(msg.local_lin_x ** 2 + msg.local_lin_y ** 2 + msg.local_lin_z ** 2)
    vir_leader_state.airspeed = airspeed
    
    # 虚拟领导者的global速度等于local速度
    vir_leader_state.global_vel_x = vir_leader_state.ned_vel_x
    vir_leader_state.global_vel_y = vir_leader_state.ned_vel_y
    vir_leader_state.global_vel_z = vir_leader_state.ned_vel_z
    
    vir_leader_state.yaw_angle = msg.deg_compass
    
    pub_leader_state()

def pub_leader_state():
	global vir_leader_state
	try:
		# Try to publish leader_id + "/fixed_wing_control/leader_state"
		vir_leader_pub.publish(vir_leader_state)
		rospy.loginfo("Publishing virtual leader states\n")
		# print(vir_leader_state)

	except Exception as e:
		print(e)
  

###########################################################################################
# main process
if __name__ == '__main__':
	rospy.init_node('pack_px4_to_serial', anonymous=True)
	rospy.loginfo("sky node has been started!")
	rate = rospy.Rate(50)
 
	# cmd_sub = rospy.Subscriber("/outdoor/uav/cmd", Cmd_send, callback=cmd_cb, queue_size=1)
 
	uav0_sub = rospy.Subscriber("/outdoor/uav0/state", Uav_state, callback=uav0_state_cb, queue_size=1)

	vir_leader_pub = rospy.Publisher(leader_id + "/fixed_wing_control/leader_state", Leaderstates, queue_size=1)
 
	# while not rospy.is_shutdown():
	# 	# pub_leader_state()
	# 	rate.sleep()
     
	rospy.spin() #ensure the callback function can refresh immediately when the topic has a new message.
